{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "21db97de",
      "metadata": {},
      "source": [
        "# Example of using Bearing Range in 3D\n",
        "\n",
        "This notebook has a very simple demonstration of locating a landmark from a moving platform. We will use a drone example."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "153c8385",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,\n",
        "Atlanta, Georgia 30332-0415\n",
        "All Rights Reserved\n",
        "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
        "\n",
        "See LICENSE for the license information\n",
        "\n",
        "Simple robotics example using odometry measurements and bearing-range (laser) measurements\n",
        "Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "3ced5f44",
      "metadata": {
        "tags": [
          "remove-input"
        ]
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/PlanarSLAMExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "158d5502",
      "metadata": {},
      "outputs": [],
      "source": [
        "import graphviz\n",
        "import numpy as np\n",
        "import plotly.graph_objects as go\n",
        "from gtsam.symbol_shorthand import L, X\n",
        "\n",
        "import gtsam\n",
        "from gtsam import Point3, Pose3, Rot3"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "d2980e5e",
      "metadata": {},
      "source": [
        "## Setting up a Small Scenario\n",
        "\n",
        "It pays off to be precise about coordinate frames.\n",
        "\n",
        "Suppose the drone is flying straight at an altitude of 200 meters, with a velocity of 10 meters per second. We'll use an FLU (forward, left, up) body frame in an ENU (north, east, up) navigation frame. Here are three successive poses at intervals of 10 seconds:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "d32414b0",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Pose 1: R: [\n",
            "\t0, -1, 0;\n",
            "\t1, -0, 0;\n",
            "\t0, -0, 1\n",
            "]\n",
            "t:   0   0 200\n",
            "\n",
            "Pose 2: R: [\n",
            "\t0, -1, 0;\n",
            "\t1, -0, 0;\n",
            "\t0, -0, 1\n",
            "]\n",
            "t:   0 100 200\n",
            "\n",
            "Pose 3: R: [\n",
            "\t0, -1, 0;\n",
            "\t1, -0, 0;\n",
            "\t0, -0, 1\n",
            "]\n",
            "t:   0 200 200\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Set up FLU body frame in ENU nav frame, flying North:\n",
        "east = Point3(1, 0, 0)\n",
        "north = Point3(0, 1, 0)\n",
        "up = Point3(0, 0, 1)\n",
        "nRb = Rot3(north, -east, up)  # body to nav\n",
        "\n",
        "# Generate three Pose3 objects at 10-second intervals, flying straight at 200m altitude\n",
        "nT1 = Pose3(nRb, Point3(0.0, 0.0, 200.0))\n",
        "nT2 = Pose3(nRb, Point3(0.0, 100.0, 200.0))   # 10 m/s * 10 s = 100 m forward\n",
        "nT3 = Pose3(nRb, Point3(0.0, 200.0, 200.0))   # 10 m/s * 20 s = 200 m forward\n",
        "poses = [nT1, nT2, nT3]\n",
        "\n",
        "print(\"Pose 1:\", nT1)\n",
        "print(\"Pose 2:\", nT2)\n",
        "print(\"Pose 3:\", nT3)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "5f72419e",
      "metadata": {},
      "source": [
        "Now, assume a landmark is located 200 m. to the east and 100 m. to the north of our starting point:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "a7a8ca58",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Landmark: [200. 100.   0.]\n"
          ]
        }
      ],
      "source": [
        "landmark = Point3(200.0, 100.0, 0.0)  # landmark at (200, 100, 0)\n",
        "print(\"Landmark:\", landmark)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "426def25",
      "metadata": {},
      "source": [
        "We can show the situation with plotly:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "9cc49106",
      "metadata": {
        "tags": [
          "remove-input"
        ]
      },
      "outputs": [
        {
          "data": {
            "application/vnd.plotly.v1+json": {
              "config": {
                "plotlyServerURL": "https://plot.ly"
              },
              "data": [
                {
                  "line": {
                    "color": "red",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 1 X",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    0,
                    30
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "green",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 1 Y",
                  "type": "scatter3d",
                  "x": [
                    0,
                    -30
                  ],
                  "y": [
                    0,
                    0
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "blue",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 1 Z",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    0,
                    0
                  ],
                  "z": [
                    200,
                    230
                  ]
                },
                {
                  "line": {
                    "color": "red",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 2 X",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    100,
                    130
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "green",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 2 Y",
                  "type": "scatter3d",
                  "x": [
                    0,
                    -30
                  ],
                  "y": [
                    100,
                    100
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "blue",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 2 Z",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    100,
                    100
                  ],
                  "z": [
                    200,
                    230
                  ]
                },
                {
                  "line": {
                    "color": "red",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 3 X",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    200,
                    230
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "green",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 3 Y",
                  "type": "scatter3d",
                  "x": [
                    0,
                    -30
                  ],
                  "y": [
                    200,
                    200
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "blue",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 3 Z",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    200,
                    200
                  ],
                  "z": [
                    200,
                    230
                  ]
                },
                {
                  "marker": {
                    "color": "orange",
                    "size": 8,
                    "symbol": "diamond"
                  },
                  "mode": "markers",
                  "name": "Landmark",
                  "type": "scatter3d",
                  "x": [
                    200
                  ],
                  "y": [
                    100
                  ],
                  "z": [
                    0
                  ]
                }
              ],
              "layout": {
                "margin": {
                  "b": 5,
                  "l": 5,
                  "r": 5,
                  "t": 25
                },
                "scene": {
                  "aspectmode": "data",
                  "xaxis": {
                    "title": {
                      "text": "X (East)"
                    }
                  },
                  "yaxis": {
                    "title": {
                      "text": "Y (North)"
                    }
                  },
                  "zaxis": {
                    "title": {
                      "text": "Z (Up)"
                    }
                  }
                },
                "showlegend": false,
                "template": {
                  "data": {
                    "bar": [
                      {
                        "error_x": {
                          "color": "#2a3f5f"
                        },
                        "error_y": {
                          "color": "#2a3f5f"
                        },
                        "marker": {
                          "line": {
                            "color": "#E5ECF6",
                            "width": 0.5
                          },
                          "pattern": {
                            "fillmode": "overlay",
                            "size": 10,
                            "solidity": 0.2
                          }
                        },
                        "type": "bar"
                      }
                    ],
                    "barpolar": [
                      {
                        "marker": {
                          "line": {
                            "color": "#E5ECF6",
                            "width": 0.5
                          },
                          "pattern": {
                            "fillmode": "overlay",
                            "size": 10,
                            "solidity": 0.2
                          }
                        },
                        "type": "barpolar"
                      }
                    ],
                    "carpet": [
                      {
                        "aaxis": {
                          "endlinecolor": "#2a3f5f",
                          "gridcolor": "white",
                          "linecolor": "white",
                          "minorgridcolor": "white",
                          "startlinecolor": "#2a3f5f"
                        },
                        "baxis": {
                          "endlinecolor": "#2a3f5f",
                          "gridcolor": "white",
                          "linecolor": "white",
                          "minorgridcolor": "white",
                          "startlinecolor": "#2a3f5f"
                        },
                        "type": "carpet"
                      }
                    ],
                    "choropleth": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "type": "choropleth"
                      }
                    ],
                    "contour": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "contour"
                      }
                    ],
                    "contourcarpet": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "type": "contourcarpet"
                      }
                    ],
                    "heatmap": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "heatmap"
                      }
                    ],
                    "histogram": [
                      {
                        "marker": {
                          "pattern": {
                            "fillmode": "overlay",
                            "size": 10,
                            "solidity": 0.2
                          }
                        },
                        "type": "histogram"
                      }
                    ],
                    "histogram2d": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "histogram2d"
                      }
                    ],
                    "histogram2dcontour": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "histogram2dcontour"
                      }
                    ],
                    "mesh3d": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "type": "mesh3d"
                      }
                    ],
                    "parcoords": [
                      {
                        "line": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "parcoords"
                      }
                    ],
                    "pie": [
                      {
                        "automargin": true,
                        "type": "pie"
                      }
                    ],
                    "scatter": [
                      {
                        "fillpattern": {
                          "fillmode": "overlay",
                          "size": 10,
                          "solidity": 0.2
                        },
                        "type": "scatter"
                      }
                    ],
                    "scatter3d": [
                      {
                        "line": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatter3d"
                      }
                    ],
                    "scattercarpet": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattercarpet"
                      }
                    ],
                    "scattergeo": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattergeo"
                      }
                    ],
                    "scattergl": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattergl"
                      }
                    ],
                    "scattermap": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattermap"
                      }
                    ],
                    "scattermapbox": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattermapbox"
                      }
                    ],
                    "scatterpolar": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatterpolar"
                      }
                    ],
                    "scatterpolargl": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatterpolargl"
                      }
                    ],
                    "scatterternary": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatterternary"
                      }
                    ],
                    "surface": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "surface"
                      }
                    ],
                    "table": [
                      {
                        "cells": {
                          "fill": {
                            "color": "#EBF0F8"
                          },
                          "line": {
                            "color": "white"
                          }
                        },
                        "header": {
                          "fill": {
                            "color": "#C8D4E3"
                          },
                          "line": {
                            "color": "white"
                          }
                        },
                        "type": "table"
                      }
                    ]
                  },
                  "layout": {
                    "annotationdefaults": {
                      "arrowcolor": "#2a3f5f",
                      "arrowhead": 0,
                      "arrowwidth": 1
                    },
                    "autotypenumbers": "strict",
                    "coloraxis": {
                      "colorbar": {
                        "outlinewidth": 0,
                        "ticks": ""
                      }
                    },
                    "colorscale": {
                      "diverging": [
                        [
                          0,
                          "#8e0152"
                        ],
                        [
                          0.1,
                          "#c51b7d"
                        ],
                        [
                          0.2,
                          "#de77ae"
                        ],
                        [
                          0.3,
                          "#f1b6da"
                        ],
                        [
                          0.4,
                          "#fde0ef"
                        ],
                        [
                          0.5,
                          "#f7f7f7"
                        ],
                        [
                          0.6,
                          "#e6f5d0"
                        ],
                        [
                          0.7,
                          "#b8e186"
                        ],
                        [
                          0.8,
                          "#7fbc41"
                        ],
                        [
                          0.9,
                          "#4d9221"
                        ],
                        [
                          1,
                          "#276419"
                        ]
                      ],
                      "sequential": [
                        [
                          0,
                          "#0d0887"
                        ],
                        [
                          0.1111111111111111,
                          "#46039f"
                        ],
                        [
                          0.2222222222222222,
                          "#7201a8"
                        ],
                        [
                          0.3333333333333333,
                          "#9c179e"
                        ],
                        [
                          0.4444444444444444,
                          "#bd3786"
                        ],
                        [
                          0.5555555555555556,
                          "#d8576b"
                        ],
                        [
                          0.6666666666666666,
                          "#ed7953"
                        ],
                        [
                          0.7777777777777778,
                          "#fb9f3a"
                        ],
                        [
                          0.8888888888888888,
                          "#fdca26"
                        ],
                        [
                          1,
                          "#f0f921"
                        ]
                      ],
                      "sequentialminus": [
                        [
                          0,
                          "#0d0887"
                        ],
                        [
                          0.1111111111111111,
                          "#46039f"
                        ],
                        [
                          0.2222222222222222,
                          "#7201a8"
                        ],
                        [
                          0.3333333333333333,
                          "#9c179e"
                        ],
                        [
                          0.4444444444444444,
                          "#bd3786"
                        ],
                        [
                          0.5555555555555556,
                          "#d8576b"
                        ],
                        [
                          0.6666666666666666,
                          "#ed7953"
                        ],
                        [
                          0.7777777777777778,
                          "#fb9f3a"
                        ],
                        [
                          0.8888888888888888,
                          "#fdca26"
                        ],
                        [
                          1,
                          "#f0f921"
                        ]
                      ]
                    },
                    "colorway": [
                      "#636efa",
                      "#EF553B",
                      "#00cc96",
                      "#ab63fa",
                      "#FFA15A",
                      "#19d3f3",
                      "#FF6692",
                      "#B6E880",
                      "#FF97FF",
                      "#FECB52"
                    ],
                    "font": {
                      "color": "#2a3f5f"
                    },
                    "geo": {
                      "bgcolor": "white",
                      "lakecolor": "white",
                      "landcolor": "#E5ECF6",
                      "showlakes": true,
                      "showland": true,
                      "subunitcolor": "white"
                    },
                    "hoverlabel": {
                      "align": "left"
                    },
                    "hovermode": "closest",
                    "mapbox": {
                      "style": "light"
                    },
                    "paper_bgcolor": "white",
                    "plot_bgcolor": "#E5ECF6",
                    "polar": {
                      "angularaxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      },
                      "bgcolor": "#E5ECF6",
                      "radialaxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      }
                    },
                    "scene": {
                      "xaxis": {
                        "backgroundcolor": "#E5ECF6",
                        "gridcolor": "white",
                        "gridwidth": 2,
                        "linecolor": "white",
                        "showbackground": true,
                        "ticks": "",
                        "zerolinecolor": "white"
                      },
                      "yaxis": {
                        "backgroundcolor": "#E5ECF6",
                        "gridcolor": "white",
                        "gridwidth": 2,
                        "linecolor": "white",
                        "showbackground": true,
                        "ticks": "",
                        "zerolinecolor": "white"
                      },
                      "zaxis": {
                        "backgroundcolor": "#E5ECF6",
                        "gridcolor": "white",
                        "gridwidth": 2,
                        "linecolor": "white",
                        "showbackground": true,
                        "ticks": "",
                        "zerolinecolor": "white"
                      }
                    },
                    "shapedefaults": {
                      "line": {
                        "color": "#2a3f5f"
                      }
                    },
                    "ternary": {
                      "aaxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      },
                      "baxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      },
                      "bgcolor": "#E5ECF6",
                      "caxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      }
                    },
                    "title": {
                      "x": 0.05
                    },
                    "xaxis": {
                      "automargin": true,
                      "gridcolor": "white",
                      "linecolor": "white",
                      "ticks": "",
                      "title": {
                        "standoff": 15
                      },
                      "zerolinecolor": "white",
                      "zerolinewidth": 2
                    },
                    "yaxis": {
                      "automargin": true,
                      "gridcolor": "white",
                      "linecolor": "white",
                      "ticks": "",
                      "title": {
                        "standoff": 15
                      },
                      "zerolinecolor": "white",
                      "zerolinewidth": 2
                    }
                  }
                },
                "title": {
                  "text": "3D Poses, Landmark, and Bearings"
                }
              }
            }
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "source": [
        "def plot_pose3_axes(fig, pose: Pose3, name, axis_length=30):\n",
        "  \"\"\"Plot a Pose3 as three colored axes.\"\"\"\n",
        "  t = pose.translation()\n",
        "  R = pose.rotation().matrix()\n",
        "  x_axis = t + axis_length * R[:, 0]\n",
        "  y_axis = t + axis_length * R[:, 1]\n",
        "  z_axis = t + axis_length * R[:, 2]\n",
        "  # X axis (red)\n",
        "  fig.add_trace(go.Scatter3d(x=[t[0], x_axis[0]], y=[t[1], x_axis[1]], z=[t[2], x_axis[2]], mode='lines', line=dict(color='red', width=5), name=f'{name} X'))\n",
        "  fig.add_trace(go.Scatter3d(x=[t[0], y_axis[0]], y=[t[1], y_axis[1]], z=[t[2], y_axis[2]], mode='lines', line=dict(color='green', width=5), name=f'{name} Y'))\n",
        "  fig.add_trace(go.Scatter3d(x=[t[0], z_axis[0]], y=[t[1], z_axis[1]], z=[t[2], z_axis[2]], mode='lines', line=dict(color='blue', width=5), name=f'{name} Z'))\n",
        "\n",
        "fig = go.Figure()\n",
        "\n",
        "# Plot poses as coordinate frames\n",
        "for idx, pose in enumerate(poses):\n",
        "  plot_pose3_axes(fig, pose, f'Pose {idx+1}')\n",
        "\n",
        "# Plot landmark\n",
        "fig.add_trace(go.Scatter3d(\n",
        "  x=[landmark[0]], y=[landmark[1]], z=[landmark[2]],\n",
        "  mode='markers', marker=dict(size=8, color='orange', symbol='diamond'),\n",
        "  name='Landmark'))\n",
        "\n",
        "fig.update_layout(scene=dict(xaxis_title='X (East)', yaxis_title='Y (North)', zaxis_title='Z (Up)', aspectmode='data'), title='3D Poses, Landmark, and Bearings', showlegend=False)\n",
        "fig.update_layout(margin=dict(l=5, r=5, t=25, b=5))\n",
        "fig.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "48c099c0",
      "metadata": {},
      "source": [
        "## Creating Bearing-Range Measurements\n",
        "\n",
        "Because we have the ground truth poses and landmark, we can compute the measurements we will get. In GTSAM, we actually have a handy class for such measurements, `BearingRange`. The 3D variant has a static method that can compute bearing-range given a pose and a landmark:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "1cf4edb2",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Measurements (bearing, range):\n",
            "\n",
            "From Pose 1:\n",
            "Bearing (Unit3): [ 0.33333333 -0.66666667 -0.66666667], Range 300.0 (m)\n",
            "\n",
            "From Pose 2:\n",
            "Bearing (Unit3): [ 0.         -0.70710678 -0.70710678], Range 282.842712474619 (m)\n",
            "\n",
            "From Pose 3:\n",
            "Bearing (Unit3): [-0.33333333 -0.66666667 -0.66666667], Range 300.0 (m)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "measurements = [gtsam.BearingRange3D.Measure(pose, landmark) for pose in poses]\n",
        "print(\"Measurements (bearing, range):\\n\")\n",
        "for i, meas in enumerate(measurements):\n",
        "    print(f\"From Pose {i+1}:\\nBearing (Unit3): {meas.bearing().point3()}, Range {meas.range()} (m)\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "f05d6073",
      "metadata": {},
      "source": [
        "Note that the bearing measurements are of type `gtsam.Unit3`, i.e., 3D unit vectors lying on a sphere of radius $1.0$. This manifold is the right representations of directions in space.\n",
        "\n",
        "Here is the situation plotted. Note that we have to convert bearing vectors into nav frame, and we'll scale them with range:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "1f350495",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Sight Vectors (in nav frame):\n",
            "\n",
            "From Pose 1: [ 200.  100. -200.]\n",
            "\n",
            "From Pose 2: [ 200.    0. -200.]\n",
            "\n",
            "From Pose 3: [ 200. -100. -200.]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "sight_vectors = [meas.range()*nRb.rotate(meas.bearing().point3()) for meas in measurements]\n",
        "print(\"Sight Vectors (in nav frame):\\n\")\n",
        "for i, vec in enumerate(sight_vectors):\n",
        "    print(f\"From Pose {i+1}: {vec}\\n\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "id": "9a9e9bd1",
      "metadata": {
        "tags": [
          "remove-input"
        ]
      },
      "outputs": [
        {
          "data": {
            "application/vnd.plotly.v1+json": {
              "config": {
                "plotlyServerURL": "https://plot.ly"
              },
              "data": [
                {
                  "line": {
                    "color": "red",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 1 X",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    0,
                    30
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "green",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 1 Y",
                  "type": "scatter3d",
                  "x": [
                    0,
                    -30
                  ],
                  "y": [
                    0,
                    0
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "blue",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 1 Z",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    0,
                    0
                  ],
                  "z": [
                    200,
                    230
                  ]
                },
                {
                  "line": {
                    "color": "red",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 2 X",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    100,
                    130
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "green",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 2 Y",
                  "type": "scatter3d",
                  "x": [
                    0,
                    -30
                  ],
                  "y": [
                    100,
                    100
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "blue",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 2 Z",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    100,
                    100
                  ],
                  "z": [
                    200,
                    230
                  ]
                },
                {
                  "line": {
                    "color": "red",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 3 X",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    200,
                    230
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "green",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 3 Y",
                  "type": "scatter3d",
                  "x": [
                    0,
                    -30
                  ],
                  "y": [
                    200,
                    200
                  ],
                  "z": [
                    200,
                    200
                  ]
                },
                {
                  "line": {
                    "color": "blue",
                    "width": 5
                  },
                  "mode": "lines",
                  "name": "Pose 3 Z",
                  "type": "scatter3d",
                  "x": [
                    0,
                    0
                  ],
                  "y": [
                    200,
                    200
                  ],
                  "z": [
                    200,
                    230
                  ]
                },
                {
                  "marker": {
                    "color": "orange",
                    "size": 8,
                    "symbol": "diamond"
                  },
                  "mode": "markers",
                  "name": "Landmark",
                  "type": "scatter3d",
                  "x": [
                    200
                  ],
                  "y": [
                    100
                  ],
                  "z": [
                    0
                  ]
                },
                {
                  "line": {
                    "color": "purple",
                    "dash": "dash",
                    "width": 4
                  },
                  "mode": "lines",
                  "name": "Bearing 1",
                  "type": "scatter3d",
                  "x": [
                    0,
                    200
                  ],
                  "y": [
                    0,
                    100
                  ],
                  "z": [
                    200,
                    0
                  ]
                },
                {
                  "line": {
                    "color": "purple",
                    "dash": "dash",
                    "width": 4
                  },
                  "mode": "lines",
                  "name": "Bearing 2",
                  "type": "scatter3d",
                  "x": [
                    0,
                    200
                  ],
                  "y": [
                    100,
                    100
                  ],
                  "z": [
                    200,
                    0
                  ]
                },
                {
                  "line": {
                    "color": "purple",
                    "dash": "dash",
                    "width": 4
                  },
                  "mode": "lines",
                  "name": "Bearing 3",
                  "type": "scatter3d",
                  "x": [
                    0,
                    200
                  ],
                  "y": [
                    200,
                    100
                  ],
                  "z": [
                    200,
                    0
                  ]
                }
              ],
              "layout": {
                "margin": {
                  "b": 5,
                  "l": 5,
                  "r": 5,
                  "t": 25
                },
                "scene": {
                  "aspectmode": "data",
                  "xaxis": {
                    "title": {
                      "text": "X (East)"
                    }
                  },
                  "yaxis": {
                    "title": {
                      "text": "Y (North)"
                    }
                  },
                  "zaxis": {
                    "title": {
                      "text": "Z (Up)"
                    }
                  }
                },
                "showlegend": false,
                "template": {
                  "data": {
                    "bar": [
                      {
                        "error_x": {
                          "color": "#2a3f5f"
                        },
                        "error_y": {
                          "color": "#2a3f5f"
                        },
                        "marker": {
                          "line": {
                            "color": "#E5ECF6",
                            "width": 0.5
                          },
                          "pattern": {
                            "fillmode": "overlay",
                            "size": 10,
                            "solidity": 0.2
                          }
                        },
                        "type": "bar"
                      }
                    ],
                    "barpolar": [
                      {
                        "marker": {
                          "line": {
                            "color": "#E5ECF6",
                            "width": 0.5
                          },
                          "pattern": {
                            "fillmode": "overlay",
                            "size": 10,
                            "solidity": 0.2
                          }
                        },
                        "type": "barpolar"
                      }
                    ],
                    "carpet": [
                      {
                        "aaxis": {
                          "endlinecolor": "#2a3f5f",
                          "gridcolor": "white",
                          "linecolor": "white",
                          "minorgridcolor": "white",
                          "startlinecolor": "#2a3f5f"
                        },
                        "baxis": {
                          "endlinecolor": "#2a3f5f",
                          "gridcolor": "white",
                          "linecolor": "white",
                          "minorgridcolor": "white",
                          "startlinecolor": "#2a3f5f"
                        },
                        "type": "carpet"
                      }
                    ],
                    "choropleth": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "type": "choropleth"
                      }
                    ],
                    "contour": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "contour"
                      }
                    ],
                    "contourcarpet": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "type": "contourcarpet"
                      }
                    ],
                    "heatmap": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "heatmap"
                      }
                    ],
                    "histogram": [
                      {
                        "marker": {
                          "pattern": {
                            "fillmode": "overlay",
                            "size": 10,
                            "solidity": 0.2
                          }
                        },
                        "type": "histogram"
                      }
                    ],
                    "histogram2d": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "histogram2d"
                      }
                    ],
                    "histogram2dcontour": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "histogram2dcontour"
                      }
                    ],
                    "mesh3d": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "type": "mesh3d"
                      }
                    ],
                    "parcoords": [
                      {
                        "line": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "parcoords"
                      }
                    ],
                    "pie": [
                      {
                        "automargin": true,
                        "type": "pie"
                      }
                    ],
                    "scatter": [
                      {
                        "fillpattern": {
                          "fillmode": "overlay",
                          "size": 10,
                          "solidity": 0.2
                        },
                        "type": "scatter"
                      }
                    ],
                    "scatter3d": [
                      {
                        "line": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatter3d"
                      }
                    ],
                    "scattercarpet": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattercarpet"
                      }
                    ],
                    "scattergeo": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattergeo"
                      }
                    ],
                    "scattergl": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattergl"
                      }
                    ],
                    "scattermap": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattermap"
                      }
                    ],
                    "scattermapbox": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scattermapbox"
                      }
                    ],
                    "scatterpolar": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatterpolar"
                      }
                    ],
                    "scatterpolargl": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatterpolargl"
                      }
                    ],
                    "scatterternary": [
                      {
                        "marker": {
                          "colorbar": {
                            "outlinewidth": 0,
                            "ticks": ""
                          }
                        },
                        "type": "scatterternary"
                      }
                    ],
                    "surface": [
                      {
                        "colorbar": {
                          "outlinewidth": 0,
                          "ticks": ""
                        },
                        "colorscale": [
                          [
                            0,
                            "#0d0887"
                          ],
                          [
                            0.1111111111111111,
                            "#46039f"
                          ],
                          [
                            0.2222222222222222,
                            "#7201a8"
                          ],
                          [
                            0.3333333333333333,
                            "#9c179e"
                          ],
                          [
                            0.4444444444444444,
                            "#bd3786"
                          ],
                          [
                            0.5555555555555556,
                            "#d8576b"
                          ],
                          [
                            0.6666666666666666,
                            "#ed7953"
                          ],
                          [
                            0.7777777777777778,
                            "#fb9f3a"
                          ],
                          [
                            0.8888888888888888,
                            "#fdca26"
                          ],
                          [
                            1,
                            "#f0f921"
                          ]
                        ],
                        "type": "surface"
                      }
                    ],
                    "table": [
                      {
                        "cells": {
                          "fill": {
                            "color": "#EBF0F8"
                          },
                          "line": {
                            "color": "white"
                          }
                        },
                        "header": {
                          "fill": {
                            "color": "#C8D4E3"
                          },
                          "line": {
                            "color": "white"
                          }
                        },
                        "type": "table"
                      }
                    ]
                  },
                  "layout": {
                    "annotationdefaults": {
                      "arrowcolor": "#2a3f5f",
                      "arrowhead": 0,
                      "arrowwidth": 1
                    },
                    "autotypenumbers": "strict",
                    "coloraxis": {
                      "colorbar": {
                        "outlinewidth": 0,
                        "ticks": ""
                      }
                    },
                    "colorscale": {
                      "diverging": [
                        [
                          0,
                          "#8e0152"
                        ],
                        [
                          0.1,
                          "#c51b7d"
                        ],
                        [
                          0.2,
                          "#de77ae"
                        ],
                        [
                          0.3,
                          "#f1b6da"
                        ],
                        [
                          0.4,
                          "#fde0ef"
                        ],
                        [
                          0.5,
                          "#f7f7f7"
                        ],
                        [
                          0.6,
                          "#e6f5d0"
                        ],
                        [
                          0.7,
                          "#b8e186"
                        ],
                        [
                          0.8,
                          "#7fbc41"
                        ],
                        [
                          0.9,
                          "#4d9221"
                        ],
                        [
                          1,
                          "#276419"
                        ]
                      ],
                      "sequential": [
                        [
                          0,
                          "#0d0887"
                        ],
                        [
                          0.1111111111111111,
                          "#46039f"
                        ],
                        [
                          0.2222222222222222,
                          "#7201a8"
                        ],
                        [
                          0.3333333333333333,
                          "#9c179e"
                        ],
                        [
                          0.4444444444444444,
                          "#bd3786"
                        ],
                        [
                          0.5555555555555556,
                          "#d8576b"
                        ],
                        [
                          0.6666666666666666,
                          "#ed7953"
                        ],
                        [
                          0.7777777777777778,
                          "#fb9f3a"
                        ],
                        [
                          0.8888888888888888,
                          "#fdca26"
                        ],
                        [
                          1,
                          "#f0f921"
                        ]
                      ],
                      "sequentialminus": [
                        [
                          0,
                          "#0d0887"
                        ],
                        [
                          0.1111111111111111,
                          "#46039f"
                        ],
                        [
                          0.2222222222222222,
                          "#7201a8"
                        ],
                        [
                          0.3333333333333333,
                          "#9c179e"
                        ],
                        [
                          0.4444444444444444,
                          "#bd3786"
                        ],
                        [
                          0.5555555555555556,
                          "#d8576b"
                        ],
                        [
                          0.6666666666666666,
                          "#ed7953"
                        ],
                        [
                          0.7777777777777778,
                          "#fb9f3a"
                        ],
                        [
                          0.8888888888888888,
                          "#fdca26"
                        ],
                        [
                          1,
                          "#f0f921"
                        ]
                      ]
                    },
                    "colorway": [
                      "#636efa",
                      "#EF553B",
                      "#00cc96",
                      "#ab63fa",
                      "#FFA15A",
                      "#19d3f3",
                      "#FF6692",
                      "#B6E880",
                      "#FF97FF",
                      "#FECB52"
                    ],
                    "font": {
                      "color": "#2a3f5f"
                    },
                    "geo": {
                      "bgcolor": "white",
                      "lakecolor": "white",
                      "landcolor": "#E5ECF6",
                      "showlakes": true,
                      "showland": true,
                      "subunitcolor": "white"
                    },
                    "hoverlabel": {
                      "align": "left"
                    },
                    "hovermode": "closest",
                    "mapbox": {
                      "style": "light"
                    },
                    "paper_bgcolor": "white",
                    "plot_bgcolor": "#E5ECF6",
                    "polar": {
                      "angularaxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      },
                      "bgcolor": "#E5ECF6",
                      "radialaxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      }
                    },
                    "scene": {
                      "xaxis": {
                        "backgroundcolor": "#E5ECF6",
                        "gridcolor": "white",
                        "gridwidth": 2,
                        "linecolor": "white",
                        "showbackground": true,
                        "ticks": "",
                        "zerolinecolor": "white"
                      },
                      "yaxis": {
                        "backgroundcolor": "#E5ECF6",
                        "gridcolor": "white",
                        "gridwidth": 2,
                        "linecolor": "white",
                        "showbackground": true,
                        "ticks": "",
                        "zerolinecolor": "white"
                      },
                      "zaxis": {
                        "backgroundcolor": "#E5ECF6",
                        "gridcolor": "white",
                        "gridwidth": 2,
                        "linecolor": "white",
                        "showbackground": true,
                        "ticks": "",
                        "zerolinecolor": "white"
                      }
                    },
                    "shapedefaults": {
                      "line": {
                        "color": "#2a3f5f"
                      }
                    },
                    "ternary": {
                      "aaxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      },
                      "baxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      },
                      "bgcolor": "#E5ECF6",
                      "caxis": {
                        "gridcolor": "white",
                        "linecolor": "white",
                        "ticks": ""
                      }
                    },
                    "title": {
                      "x": 0.05
                    },
                    "xaxis": {
                      "automargin": true,
                      "gridcolor": "white",
                      "linecolor": "white",
                      "ticks": "",
                      "title": {
                        "standoff": 15
                      },
                      "zerolinecolor": "white",
                      "zerolinewidth": 2
                    },
                    "yaxis": {
                      "automargin": true,
                      "gridcolor": "white",
                      "linecolor": "white",
                      "ticks": "",
                      "title": {
                        "standoff": 15
                      },
                      "zerolinecolor": "white",
                      "zerolinewidth": 2
                    }
                  }
                },
                "title": {
                  "text": "3D Poses, Landmark, and Bearings"
                }
              }
            }
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "source": [
        "fig = go.Figure()\n",
        "\n",
        "# Plot poses as coordinate frames\n",
        "for idx, pose in enumerate(poses):\n",
        "  plot_pose3_axes(fig, pose, f'Pose {idx+1}')\n",
        "\n",
        "# Plot landmark\n",
        "fig.add_trace(go.Scatter3d(\n",
        "  x=[landmark[0]], y=[landmark[1]], z=[landmark[2]],\n",
        "  mode='markers', marker=dict(size=8, color='orange', symbol='diamond'),\n",
        "  name='Landmark'))\n",
        "\n",
        "# Plot unit vectors (bearings) from each pose to the landmark\n",
        "for idx, (pose, meas) in enumerate(zip(poses, measurements)):\n",
        "  t_k = pose.translation()\n",
        "  vec_end = t_k + sight_vectors[idx]\n",
        "  fig.add_trace(go.Scatter3d(x=[t_k[0], vec_end[0]], y=[t_k[1], vec_end[1]], z=[t_k[2], vec_end[2]], mode='lines', line=dict(color='purple', width=4, dash='dash'), name=f'Bearing {idx+1}'))\n",
        "\n",
        "fig.update_layout(scene=dict(xaxis_title='X (East)', yaxis_title='Y (North)', zaxis_title='Z (Up)', aspectmode='data'), title='3D Poses, Landmark, and Bearings', showlegend=False)\n",
        "fig.update_layout(margin=dict(l=5, r=5, t=25, b=5))\n",
        "fig.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "060574f1",
      "metadata": {},
      "source": [
        "## Sensor Fusion with GTSAM\n",
        "\n",
        "We assume the drone has a navigation system that gives us `Pose3` updates, with a covariance. We  create a factor graph with three prior factors to provide that information:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "id": "cb3d5141",
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create a NonlinearFactorGraph with three Pose3 prior factors at different locations.\n",
        "graph3d = gtsam.NonlinearFactorGraph()\n",
        "\n",
        "sigma_degree = 5\n",
        "sigma_meter = 0.5\n",
        "sigmas:np.ndarray = np.array([np.radians(sigma_degree)] * 3 + [sigma_meter] * 3)\n",
        "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(sigmas)\n",
        "for idx, nTk in enumerate(poses):\n",
        "    graph3d.addPriorPose3(X(idx+1), nTk, PRIOR_NOISE)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "b9efa777",
      "metadata": {},
      "source": [
        "We then have three bearing range factors, created like this"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "id": "1301ff6a",
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create a noise model (isotropic, 3D, sigma=0.5)\n",
        "model3D = gtsam.noiseModel.Isotropic.Sigma(3, 0.5)\n",
        "\n",
        "# Create the 3D bearing-range factors\n",
        "for idx, meas in enumerate(measurements):\n",
        "    factor = gtsam.BearingRangeFactor3D(X(idx+1), L(1), meas.bearing(), meas.range(), model3D)\n",
        "    graph3d.add(factor)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "080524e8",
      "metadata": {},
      "source": [
        "We wil set up an optimization problem starting from *completely wrong* values:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "id": "40089be2",
      "metadata": {},
      "outputs": [],
      "source": [
        "# Set up values at the linearization point\n",
        "initial_estimate = gtsam.Values()\n",
        "for k, _ in enumerate(poses, 1):\n",
        "  initial_estimate.insert(X(k), Pose3())\n",
        "initial_estimate.insert(L(1), Point3(1,2,3))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "id": "d896ecee",
      "metadata": {},
      "outputs": [
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"278pt\" height=\"84pt\"\n",
              " viewBox=\"0.00 0.00 278.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 274,-79.6 274,4 -4,4\"/>\n",
              "<!-- var7782220156096217089 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var7782220156096217089</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">l1</text>\n",
              "</g>\n",
              "<!-- factor3 -->\n",
              "<g id=\"node8\" class=\"node\">\n",
              "<title>factor3</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"81\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7782220156096217089&#45;&#45;factor3 -->\n",
              "<g id=\"edge5\" class=\"edge\">\n",
              "<title>var7782220156096217089&#45;&#45;factor3</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M93.3,-39.58C88.77,-26.03 82.94,-8.61 81.39,-3.97\"/>\n",
              "</g>\n",
              "<!-- factor4 -->\n",
              "<g id=\"node9\" class=\"node\">\n",
              "<title>factor4</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"117\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7782220156096217089&#45;&#45;factor4 -->\n",
              "<g id=\"edge7\" class=\"edge\">\n",
              "<title>var7782220156096217089&#45;&#45;factor4</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M104.7,-39.58C109.23,-26.03 115.06,-8.61 116.61,-3.97\"/>\n",
              "</g>\n",
              "<!-- factor5 -->\n",
              "<g id=\"node10\" class=\"node\">\n",
              "<title>factor5</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"207\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7782220156096217089&#45;&#45;factor5 -->\n",
              "<g id=\"edge9\" class=\"edge\">\n",
              "<title>var7782220156096217089&#45;&#45;factor5</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M120.34,-45.97C148.86,-31.76 196.86,-7.85 205.61,-3.49\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node5\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"27\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M27,-39.28C27,-25.77 27,-8.54 27,-3.96\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor3 -->\n",
              "<g id=\"edge4\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor3</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M41.47,-42.18C55.47,-28.24 75.23,-8.54 79.96,-3.84\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352322 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>var8646911284551352322</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x2</text>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node6\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"171\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352322&#45;&#45;factor1 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var8646911284551352322&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M171,-39.28C171,-25.77 171,-8.54 171,-3.96\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352322&#45;&#45;factor4 -->\n",
              "<g id=\"edge6\" class=\"edge\">\n",
              "<title>var8646911284551352322&#45;&#45;factor4</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M156.53,-42.18C142.53,-28.24 122.77,-8.54 118.04,-3.84\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352323 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>var8646911284551352323</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"243\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"243\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x3</text>\n",
              "</g>\n",
              "<!-- factor2 -->\n",
              "<g id=\"node7\" class=\"node\">\n",
              "<title>factor2</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"243\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352323&#45;&#45;factor2 -->\n",
              "<g id=\"edge3\" class=\"edge\">\n",
              "<title>var8646911284551352323&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M243,-39.28C243,-25.77 243,-8.54 243,-3.96\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352323&#45;&#45;factor5 -->\n",
              "<g id=\"edge8\" class=\"edge\">\n",
              "<title>var8646911284551352323&#45;&#45;factor5</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M232.39,-40.75C223.1,-26.86 210.63,-8.22 207.66,-3.78\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x118659760>"
            ]
          },
          "metadata": {},
          "output_type": "display_data"
        }
      ],
      "source": [
        "display(graphviz.Source(graph3d.dot(initial_estimate)))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "2608bf96",
      "metadata": {},
      "source": [
        "Now, we use an optimizer to find the variable configuration that best fits all the factors (measurements) in the graph, starting from the initial estimate.\n",
        "\n",
        "We'll use the Levenberg-Marquardt (LM) algorithm, a standard non-linear least-squares optimizer.\n",
        "\n",
        "1.  Create LM parameters (`gtsam.LevenbergMarquardtParams`). We'll use the defaults.\n",
        "2.  Create the optimizer instance, providing the graph, initial estimate, and parameters.\n",
        "3.  Run the optimization by calling `optimizer.optimize()`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "id": "2ee6b17a",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initial error: 870772, values: 4\n",
            "iter      cost      cost_change    lambda  success iter_time\n",
            "   0        29705      8.4e+05      1e-05      1          0\n",
            "   1      1.6e+07     -1.6e+07      1e-06      1          0\n",
            "   1      1.4e+07     -1.4e+07      1e-05      1          0\n",
            "   1      6.1e+06     -6.1e+06     0.0001      1          0\n",
            "   1      6.1e+05     -5.8e+05      0.001      1          0\n",
            "   1      1.8e+05     -1.5e+05       0.01      1          0\n",
            "   1      7.3e+03      2.2e+04        0.1      1          0\n",
            "   2      2.5e+02        7e+03       0.01      1          0\n",
            "   3      1.1e+02      1.4e+02      0.001      1          0\n",
            "   4        3e+04       -3e+04     0.0001      1          0\n",
            "   4           73           41      0.001      1          0\n",
            "   5        2e+04       -2e+04     0.0001      1          0\n",
            "   5           48           25      0.001      1          0\n",
            "   6      1.3e+04     -1.3e+04     0.0001      1          0\n",
            "   6           32           17      0.001      1          0\n",
            "   7      8.3e+03     -8.3e+03     0.0001      1          0\n",
            "   7           21           11      0.001      1          0\n",
            "   8      5.3e+03     -5.2e+03     0.0001      1          0\n",
            "   8           13          7.2      0.001      1          0\n",
            "   9      3.3e+03     -3.3e+03     0.0001      1          0\n",
            "   9          8.7          4.6      0.001      1          0\n",
            "  10        2e+03       -2e+03     0.0001      1          0\n",
            "  10          5.7            3      0.001      1          0\n",
            "  11      1.3e+03     -1.3e+03     0.0001      1          0\n",
            "  11          3.8          1.9      0.001      1          0\n",
            "  12      7.8e+02     -7.7e+02     0.0001      1          0\n",
            "  12          2.6          1.2      0.001      1          0\n",
            "  13      4.7e+02     -4.7e+02     0.0001      1          0\n",
            "  13          1.7         0.82      0.001      1          0\n",
            "  14      2.9e+02     -2.9e+02     0.0001      1          0\n",
            "  14          1.2         0.54      0.001      1          0\n",
            "  15      1.8e+02     -1.7e+02     0.0001      1          0\n",
            "  15         0.84         0.36      0.001      1          0\n",
            "  16      1.1e+02     -1.1e+02     0.0001      1          0\n",
            "  16          0.6         0.24      0.001      1          0\n",
            "  17           65          -64     0.0001      1          0\n",
            "  17         0.43         0.17      0.001      1          0\n",
            "  18           39          -39     0.0001      1          0\n",
            "  18         0.31         0.12      0.001      1          0\n",
            "  19           24          -23     0.0001      1          0\n",
            "  19         0.23        0.082      0.001      1          0\n",
            "  20           14          -14     0.0001      1          0\n",
            "  20         0.17        0.059      0.001      1          0\n",
            "  21          8.7         -8.5     0.0001      1          0\n",
            "  21         0.13        0.043      0.001      1          0\n",
            "  22          5.3         -5.1     0.0001      1          0\n",
            "  22        0.097        0.031      0.001      1          0\n",
            "  23          3.2         -3.1     0.0001      1          0\n",
            "  23        0.074        0.023      0.001      1          0\n",
            "  24          1.9         -1.8     0.0001      1          0\n",
            "  24        0.056        0.017      0.001      1          0\n",
            "  25          1.2         -1.1     0.0001      1          0\n",
            "  25        0.043        0.013      0.001      1          0\n",
            "  26          0.7        -0.66     0.0001      1          0\n",
            "  26        0.033         0.01      0.001      1          0\n",
            "  27         0.42        -0.39     0.0001      1          0\n",
            "  27        0.025       0.0076      0.001      1          0\n",
            "  28         0.26        -0.23     0.0001      1          0\n",
            "  28         0.02       0.0058      0.001      1          0\n",
            "  29         0.16        -0.14     0.0001      1          0\n",
            "  29        0.015       0.0045      0.001      1          0\n",
            "  30        0.095       -0.079     0.0001      1          0\n",
            "  30        0.012       0.0034      0.001      1          0\n",
            "  31        0.058       -0.046     0.0001      1          0\n",
            "  31       0.0091       0.0026      0.001      1          0\n",
            "  32        0.035       -0.026     0.0001      1          0\n",
            "  32        0.007        0.002      0.001      1          0\n",
            "  33        0.021       -0.014     0.0001      1          0\n",
            "  33       0.0055       0.0016      0.001      1          0\n",
            "  34        0.013      -0.0077     0.0001      1          0\n",
            "  34       0.0042       0.0012      0.001      1          0\n",
            "  35       0.0081      -0.0039     0.0001      1          0\n",
            "  35       0.0033      0.00095      0.001      1          0\n",
            "  36        0.005      -0.0017     0.0001      1          0\n",
            "  36       0.0025      0.00073      0.001      1          0\n",
            "  37       0.0031     -0.00058     0.0001      1          0\n",
            "  37        0.002      0.00057      0.001      1          0\n",
            "  38        0.002      7.4e-06     0.0001      1          0\n"
          ]
        }
      ],
      "source": [
        "# Optimize using Levenberg-Marquardt optimization.\n",
        "# The optimizer accepts optional parameters, but we'll use the defaults here.\n",
        "params = gtsam.LevenbergMarquardtParams()\n",
        "params.setVerbosityLM(\"SUMMARY\")\n",
        "optimizer = gtsam.LevenbergMarquardtOptimizer(graph3d, initial_estimate, params)\n",
        "\n",
        "# Perform the optimization\n",
        "result = optimizer.optimize()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "6111efa9",
      "metadata": {},
      "source": [
        "As you can see, and not surprising as we used the ground truth measurements without adding noise, the error is optimized to zero. Note this is a very non-linear objective function, and hence it takes a few iterations to converge. However, this would be much faster with a good initial estimate.\n",
        "\n",
        "The recovered poses and landmarks are here:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "id": "1864e025",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Final Result:\n",
            "Values with 4 values:\n",
            "Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t2e+02;\n",
            "\t1e+02;\n",
            "\t1.6\n",
            "]\n",
            "\n",
            "Value x1: (gtsam::Pose3)\n",
            "R: [\n",
            "\t5.4e-05, -1, -0.00021;\n",
            "\t1, 5.4e-05, -5.3e-05;\n",
            "\t5.3e-05, -0.00021, 1\n",
            "]\n",
            "t:  1.7e-05 -1.9e-07    2e+02\n",
            "\n",
            "Value x2: (gtsam::Pose3)\n",
            "R: [\n",
            "\t-1.1e-16, -1, -0.00024;\n",
            "\t1, 1e-16, -7.2e-18;\n",
            "\t2.5e-19, -0.00024, 1\n",
            "]\n",
            "t: 1.9e-05   1e+02   2e+02\n",
            "\n",
            "Value x3: (gtsam::Pose3)\n",
            "R: [\n",
            "\t-5.4e-05, -1, -0.00021;\n",
            "\t1, -5.4e-05, 5.3e-05;\n",
            "\t-5.3e-05, -0.00021, 1\n",
            "]\n",
            "t: 1.7e-05   2e+02   2e+02\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Print the final optimized result\n",
        "# This gtsam.Values object contains the most likely estimates for all variables.\n",
        "print(\"\\nFinal Result:\\n{}\".format(result))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "74673b3d",
      "metadata": {},
      "source": [
        "## Summary\n",
        "\n",
        "We have successfully:\n",
        "1.  Set up a small ground truth scenario.\n",
        "2.  Created (exact) bearing range measurements, in the body frame.\n",
        "3.  Showed th situation graphically, after rotating bearing-range vectors into nav.\n",
        "3.  Represented this problem as a `gtsam.NonlinearFactorGraph`.\n",
        "4.  Provided a very wrong initial estimate.\n",
        "5.  Used `gtsam.LevenbergMarquardtOptimizer` to recover the exact ground truth.\n",
        "\n",
        "Hopefully this is useful in using bearing-range-like sensors in your own robotics scenario."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}